/*

	FFT library
	Copyright (C) 2010 Didier Longueville
	Copyright (C) 2014 Enrique Condes

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifndef ArduinoFFT_h /* Prevent loading library twice */
#define ArduinoFFT_h
#ifdef ARDUINO
#if ARDUINO >= 10
#else
#include "WProgram.h" /* This is where the standard Arduino code lies */
#endif
#else
#include <stdio.h>
#include <stdlib.h>

#ifdef __AVR__
#include <avr/io.h>
#include <avr/pgmspace.h>
#endif
#include "defs.h"
#include "types.h"
#include <math.h>

#endif

// Define this to use a low-precision square root approximation instead of the
// regular sqrt() call
// This might only work for specific use cases, but is significantly faster.
// Only works for ArduinoFFT<float>.
// #define FFT_SQRT_APPROXIMATION

#ifdef FFT_SQRT_APPROXIMATION
#include <type_traits>
#else
#define sqrt_internal sqrt
#endif

enum class FFTDirection { Reverse, Forward };

enum class FFTWindow {
  Rectangle,        // rectangle (Box car)
  Hamming,          // hamming
  Hann,             // hann
  Triangle,         // triangle (Bartlett)
  Nuttall,          // nuttall
  Blackman,         // blackman
  Blackman_Nuttall, // blackman nuttall
  Blackman_Harris,  // blackman harris
  Flat_top,         // flat top
  Welch             // welch
};
#define FFT_LIB_REV 0x15
/* Custom constants */
#define FFT_FORWARD FFTDirection::Forward
#define FFT_REVERSE FFTDirection::Reverse

/* Windowing type */
#define FFT_WIN_TYP_RECTANGLE FFTWindow::Rectangle /* rectangle (Box car) */
#define FFT_WIN_TYP_HAMMING FFTWindow::Hamming     /* hamming */
#define FFT_WIN_TYP_HANN FFTWindow::Hann           /* hann */
#define FFT_WIN_TYP_TRIANGLE FFTWindow::Triangle   /* triangle (Bartlett) */
#define FFT_WIN_TYP_NUTTALL FFTWindow::Nuttall     /* nuttall */
#define FFT_WIN_TYP_BLACKMAN FFTWindow::Blackman   /* blackman */
#define FFT_WIN_TYP_BLACKMAN_NUTTALL                                           \
  FFTWindow::Blackman_Nuttall /* blackman nuttall */
#define FFT_WIN_TYP_BLACKMAN_HARRIS                                            \
  FFTWindow::Blackman_Harris                    /* blackman harris*/
#define FFT_WIN_TYP_FLT_TOP FFTWindow::Flat_top /* flat top */
#define FFT_WIN_TYP_WELCH FFTWindow::Welch      /* welch */
/*Mathematial constants*/
#define twoPi 6.28318531
#define fourPi 12.56637061
#define sixPi 18.84955593

#ifdef __AVR__
static const double _c1[] PROGMEM = {
    0.0000000000, 0.7071067812, 0.9238795325, 0.9807852804, 0.9951847267,
    0.9987954562, 0.9996988187, 0.9999247018, 0.9999811753, 0.9999952938,
    0.9999988235, 0.9999997059, 0.9999999265, 0.9999999816, 0.9999999954,
    0.9999999989, 0.9999999997};
static const double _c2[] PROGMEM = {
    1.0000000000, 0.7071067812, 0.3826834324, 0.1950903220, 0.0980171403,
    0.0490676743, 0.0245412285, 0.0122715383, 0.0061358846, 0.0030679568,
    0.0015339802, 0.0007669903, 0.0003834952, 0.0001917476, 0.0000958738,
    0.0000479369, 0.0000239684};
#endif
class arduinoFFT {
public:
  /* Constructor */
  arduinoFFT(void);
  arduinoFFT(double *vReal, double *vImag, uint16_t samples,
             double samplingFrequency);
  /* Destructor */
  ~arduinoFFT(void);
  /* Functions */
  uint8_t Revision(void);
  uint8_t Exponent(uint16_t value);

  void ComplexToMagnitude(double *vReal, double *vImag, uint16_t samples);
  void Compute(double *vReal, double *vImag, uint16_t samples,
               FFTDirection dir);
  void Compute(double *vReal, double *vImag, uint16_t samples, uint8_t power,
               FFTDirection dir);
  void DCRemoval(double *vData, uint16_t samples);
  double MajorPeak(double *vD, uint16_t samples, double samplingFrequency);
  void MajorPeak(double *vD, uint16_t samples, double samplingFrequency,
                 double *f, double *v);
  void Windowing(double *vData, uint16_t samples, FFTWindow windowType,
                 FFTDirection dir);

  void ComplexToMagnitude();
  void Compute(FFTDirection dir);
  void DCRemoval();
  double MajorPeak();
  void MajorPeak(double *f, double *v);
  void Windowing(FFTWindow windowType, FFTDirection dir);

  double MajorPeakParabola();

private:
  /* Variables */
  uint16_t _samples;
  double _samplingFrequency;
  double *_vReal;
  double *_vImag;
  uint8_t _power;
  /* Functions */
  void Swap(double *x, double *y);
  void Parabola(double x1, double y1, double x2, double y2, double x3,
                double y3, double *a, double *b, double *c);
};

#endif


/*

	FFT library
	Copyright (C) 2010 Didier Longueville
	Copyright (C) 2014 Enrique Condes

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/


arduinoFFT::arduinoFFT(void) { // Constructor
#warning("This method is deprecated and may be removed on future revisions.")
}

arduinoFFT::arduinoFFT(double *vReal, double *vImag, uint16_t samples,
                       double samplingFrequency) { // Constructor
  this->_vReal = vReal;
  this->_vImag = vImag;
  this->_samples = samples;
  this->_samplingFrequency = samplingFrequency;
  this->_power = Exponent(samples);
}

arduinoFFT::~arduinoFFT(void) {
  // Destructor
}

uint8_t arduinoFFT::Revision(void) { return (FFT_LIB_REV); }

void arduinoFFT::Compute(double *vReal, double *vImag, uint16_t samples,
                         FFTDirection dir) {
#warning("This method is deprecated and may be removed on future revisions.")
  Compute(vReal, vImag, samples, Exponent(samples), dir);
}

void arduinoFFT::Compute(FFTDirection dir) {
  // Computes in-place complex-to-complex FFT /
  // Reverse bits /
  uint16_t j = 0;
  for (uint16_t i = 0; i < (this->_samples - 1); i++) {
    if (i < j) {
      Swap(&this->_vReal[i], &this->_vReal[j]);
      if (dir == FFT_REVERSE)
        Swap(&this->_vImag[i], &this->_vImag[j]);
    }
    uint16_t k = (this->_samples >> 1);
    while (k <= j) {
      j -= k;
      k >>= 1;
    }
    j += k;
  }
// Compute the FFT
#ifdef __AVR__
  uint8_t index = 0;
#endif
  double c1 = -1.0;
  double c2 = 0.0;
  uint16_t l2 = 1;
  for (uint8_t l = 0; (l < this->_power); l++) {
    uint16_t l1 = l2;
    l2 <<= 1;
    double u1 = 1.0;
    double u2 = 0.0;
    for (j = 0; j < l1; j++) {
      for (uint16_t i = j; i < this->_samples; i += l2) {
        uint16_t i1 = i + l1;
        double t1 = u1 * this->_vReal[i1] - u2 * this->_vImag[i1];
        double t2 = u1 * this->_vImag[i1] + u2 * this->_vReal[i1];
        this->_vReal[i1] = this->_vReal[i] - t1;
        this->_vImag[i1] = this->_vImag[i] - t2;
        this->_vReal[i] += t1;
        this->_vImag[i] += t2;
      }
      double z = ((u1 * c1) - (u2 * c2));
      u2 = ((u1 * c2) + (u2 * c1));
      u1 = z;
    }
#ifdef __AVR__
    c2 = pgm_read_float_near(&(_c2[index]));
    c1 = pgm_read_float_near(&(_c1[index]));
    index++;
#else
    c2 = sqrt((1.0 - c1) / 2.0);
    c1 = sqrt((1.0 + c1) / 2.0);
#endif
    if (dir == FFT_FORWARD) {
      c2 = -c2;
    }
  }
  // Scaling for reverse transform /
  if (dir != FFT_FORWARD) {
    double reciprocal = 1.0 / this->_samples;
    for (uint16_t i = 0; i < this->_samples; i++) {
      this->_vReal[i] *= reciprocal;
      this->_vImag[i] *= reciprocal;
    }
  }
}

void arduinoFFT::Compute(double *vReal, double *vImag, uint16_t samples,
                         uint8_t power, FFTDirection dir) {
// Computes in-place complex-to-complex FFT
// Reverse bits
#warning("This method is deprecated and may be removed on future revisions.")
  uint16_t j = 0;
  for (uint16_t i = 0; i < (samples - 1); i++) {
    if (i < j) {
      Swap(&vReal[i], &vReal[j]);
      if (dir == FFT_REVERSE)
        Swap(&vImag[i], &vImag[j]);
    }
    uint16_t k = (samples >> 1);
    while (k <= j) {
      j -= k;
      k >>= 1;
    }
    j += k;
  }
// Compute the FFT
#ifdef __AVR__
  uint8_t index = 0;
#endif
  double c1 = -1.0;
  double c2 = 0.0;
  uint16_t l2 = 1;
  for (uint8_t l = 0; (l < power); l++) {
    uint16_t l1 = l2;
    l2 <<= 1;
    double u1 = 1.0;
    double u2 = 0.0;
    for (j = 0; j < l1; j++) {
      for (uint16_t i = j; i < samples; i += l2) {
        uint16_t i1 = i + l1;
        double t1 = u1 * vReal[i1] - u2 * vImag[i1];
        double t2 = u1 * vImag[i1] + u2 * vReal[i1];
        vReal[i1] = vReal[i] - t1;
        vImag[i1] = vImag[i] - t2;
        vReal[i] += t1;
        vImag[i] += t2;
      }
      double z = ((u1 * c1) - (u2 * c2));
      u2 = ((u1 * c2) + (u2 * c1));
      u1 = z;
    }
#ifdef __AVR__
    c2 = pgm_read_float_near(&(_c2[index]));
    c1 = pgm_read_float_near(&(_c1[index]));
    index++;
#else
    c2 = sqrt((1.0 - c1) / 2.0);
    c1 = sqrt((1.0 + c1) / 2.0);
#endif
    if (dir == FFT_FORWARD) {
      c2 = -c2;
    }
  }
  // Scaling for reverse transform
  if (dir != FFT_FORWARD) {
    double reciprocal = 1.0 / samples;
    for (uint16_t i = 0; i < samples; i++) {
      vReal[i] *= reciprocal;
      vImag[i] *= reciprocal;
    }
  }
}

void arduinoFFT::ComplexToMagnitude() {
  // vM is half the size of vReal and vImag
  for (uint16_t i = 0; i < this->_samples; i++) {
    this->_vReal[i] = sqrt(sq(this->_vReal[i]) + sq(this->_vImag[i]));
  }
}

void arduinoFFT::ComplexToMagnitude(double *vReal, double *vImag,
                                    uint16_t samples) {
// vM is half the size of vReal and vImag
#warning("This method is deprecated and may be removed on future revisions.")
  for (uint16_t i = 0; i < samples; i++) {
    vReal[i] = sqrt(sq(vReal[i]) + sq(vImag[i]));
  }
}

void arduinoFFT::DCRemoval() {
  // calculate the mean of vData
  double mean = 0;
  for (uint16_t i = 0; i < this->_samples; i++) {
    mean += this->_vReal[i];
  }
  mean /= this->_samples;
  // Subtract the mean from vData
  for (uint16_t i = 0; i < this->_samples; i++) {
    this->_vReal[i] -= mean;
  }
}

void arduinoFFT::DCRemoval(double *vData, uint16_t samples) {
// calculate the mean of vData
#warning("This method is deprecated and may be removed on future revisions.")
  double mean = 0;
  for (uint16_t i = 0; i < samples; i++) {
    mean += vData[i];
  }
  mean /= samples;
  // Subtract the mean from vData
  for (uint16_t i = 0; i < samples; i++) {
    vData[i] -= mean;
  }
}

void arduinoFFT::Windowing(FFTWindow windowType, FFTDirection dir) {
  // Weighing factors are computed once before multiple use of FFT
  // The weighing function is symmetric; half the weighs are recorded
  double samplesMinusOne = (double(this->_samples) - 1.0);
  for (uint16_t i = 0; i < (this->_samples >> 1); i++) {
    double indexMinusOne = double(i);
    double ratio = (indexMinusOne / samplesMinusOne);
    double weighingFactor = 1.0;
    // Compute and record weighting factor
    switch (windowType) {
    case FFT_WIN_TYP_RECTANGLE: // rectangle (box car)
      weighingFactor = 1.0;
      break;
    case FFT_WIN_TYP_HAMMING: // hamming
      weighingFactor = 0.54 - (0.46 * cos(twoPi * ratio));
      break;
    case FFT_WIN_TYP_HANN: // hann
      weighingFactor = 0.54 * (1.0 - cos(twoPi * ratio));
      break;
    case FFT_WIN_TYP_TRIANGLE: // triangle (Bartlett)
#if defined(ESP8266) || defined(ESP32)
      weighingFactor =
          1.0 - ((2.0 * fabs(indexMinusOne - (samplesMinusOne / 2.0))) /
                 samplesMinusOne);
#else
      weighingFactor =
          1.0 - ((2.0 * abs(indexMinusOne - (samplesMinusOne / 2.0))) /
                 samplesMinusOne);
#endif
      break;
    case FFT_WIN_TYP_NUTTALL: // nuttall
      weighingFactor = 0.355768 - (0.487396 * (cos(twoPi * ratio))) +
                       (0.144232 * (cos(fourPi * ratio))) -
                       (0.012604 * (cos(sixPi * ratio)));
      break;
    case FFT_WIN_TYP_BLACKMAN: // blackman
      weighingFactor = 0.42323 - (0.49755 * (cos(twoPi * ratio))) +
                       (0.07922 * (cos(fourPi * ratio)));
      break;
    case FFT_WIN_TYP_BLACKMAN_NUTTALL: // blackman nuttall
      weighingFactor = 0.3635819 - (0.4891775 * (cos(twoPi * ratio))) +
                       (0.1365995 * (cos(fourPi * ratio))) -
                       (0.0106411 * (cos(sixPi * ratio)));
      break;
    case FFT_WIN_TYP_BLACKMAN_HARRIS: // blackman harris
      weighingFactor = 0.35875 - (0.48829 * (cos(twoPi * ratio))) +
                       (0.14128 * (cos(fourPi * ratio))) -
                       (0.01168 * (cos(sixPi * ratio)));
      break;
    case FFT_WIN_TYP_FLT_TOP: // flat top
      weighingFactor = 0.2810639 - (0.5208972 * cos(twoPi * ratio)) +
                       (0.1980399 * cos(fourPi * ratio));
      break;
    case FFT_WIN_TYP_WELCH: // welch
      weighingFactor = 1.0 - sq((indexMinusOne - samplesMinusOne / 2.0) /
                                (samplesMinusOne / 2.0));
      break;
    }
    if (dir == FFT_FORWARD) {
      this->_vReal[i] *= weighingFactor;
      this->_vReal[this->_samples - (i + 1)] *= weighingFactor;
    } else {
      this->_vReal[i] /= weighingFactor;
      this->_vReal[this->_samples - (i + 1)] /= weighingFactor;
    }
  }
}

void arduinoFFT::Windowing(double *vData, uint16_t samples,
                           FFTWindow windowType, FFTDirection dir) {
// Weighing factors are computed once before multiple use of FFT
// The weighing function is symmetric; half the weighs are recorded
#warning("This method is deprecated and may be removed on future revisions.")
  double samplesMinusOne = (double(samples) - 1.0);
  for (uint16_t i = 0; i < (samples >> 1); i++) {
    double indexMinusOne = double(i);
    double ratio = (indexMinusOne / samplesMinusOne);
    double weighingFactor = 1.0;
    // Compute and record weighting factor
    switch (windowType) {
    case FFT_WIN_TYP_RECTANGLE: // rectangle (box car)
      weighingFactor = 1.0;
      break;
    case FFT_WIN_TYP_HAMMING: // hamming
      weighingFactor = 0.54 - (0.46 * cos(twoPi * ratio));
      break;
    case FFT_WIN_TYP_HANN: // hann
      weighingFactor = 0.54 * (1.0 - cos(twoPi * ratio));
      break;
    case FFT_WIN_TYP_TRIANGLE: // triangle (Bartlett)
#if defined(ESP8266) || defined(ESP32)
      weighingFactor =
          1.0 - ((2.0 * fabs(indexMinusOne - (samplesMinusOne / 2.0))) /
                 samplesMinusOne);
#else
      weighingFactor =
          1.0 - ((2.0 * abs(indexMinusOne - (samplesMinusOne / 2.0))) /
                 samplesMinusOne);
#endif
      break;
    case FFT_WIN_TYP_NUTTALL: // nuttall
      weighingFactor = 0.355768 - (0.487396 * (cos(twoPi * ratio))) +
                       (0.144232 * (cos(fourPi * ratio))) -
                       (0.012604 * (cos(sixPi * ratio)));
      break;
    case FFT_WIN_TYP_BLACKMAN: // blackman
      weighingFactor = 0.42323 - (0.49755 * (cos(twoPi * ratio))) +
                       (0.07922 * (cos(fourPi * ratio)));
      break;
    case FFT_WIN_TYP_BLACKMAN_NUTTALL: // blackman nuttall
      weighingFactor = 0.3635819 - (0.4891775 * (cos(twoPi * ratio))) +
                       (0.1365995 * (cos(fourPi * ratio))) -
                       (0.0106411 * (cos(sixPi * ratio)));
      break;
    case FFT_WIN_TYP_BLACKMAN_HARRIS: // blackman harris
      weighingFactor = 0.35875 - (0.48829 * (cos(twoPi * ratio))) +
                       (0.14128 * (cos(fourPi * ratio))) -
                       (0.01168 * (cos(sixPi * ratio)));
      break;
    case FFT_WIN_TYP_FLT_TOP: // flat top
      weighingFactor = 0.2810639 - (0.5208972 * cos(twoPi * ratio)) +
                       (0.1980399 * cos(fourPi * ratio));
      break;
    case FFT_WIN_TYP_WELCH: // welch
      weighingFactor = 1.0 - sq((indexMinusOne - samplesMinusOne / 2.0) /
                                (samplesMinusOne / 2.0));
      break;
    }
    if (dir == FFT_FORWARD) {
      vData[i] *= weighingFactor;
      vData[samples - (i + 1)] *= weighingFactor;
    } else {
      vData[i] /= weighingFactor;
      vData[samples - (i + 1)] /= weighingFactor;
    }
  }
}

double arduinoFFT::MajorPeak() {
  double maxY = 0;
  uint16_t IndexOfMaxY = 0;
  // If sampling_frequency = 2 * max_frequency in signal,
  // value would be stored at position samples/2
  for (uint16_t i = 1; i < ((this->_samples >> 1) + 1); i++) {
    if ((this->_vReal[i - 1] < this->_vReal[i]) &&
        (this->_vReal[i] > this->_vReal[i + 1])) {
      if (this->_vReal[i] > maxY) {
        maxY = this->_vReal[i];
        IndexOfMaxY = i;
      }
    }
  }
  double delta =
      0.5 *
      ((this->_vReal[IndexOfMaxY - 1] - this->_vReal[IndexOfMaxY + 1]) /
       (this->_vReal[IndexOfMaxY - 1] - (2.0 * this->_vReal[IndexOfMaxY]) +
        this->_vReal[IndexOfMaxY + 1]));
  double interpolatedX =
      ((IndexOfMaxY + delta) * this->_samplingFrequency) / (this->_samples - 1);
  if (IndexOfMaxY ==
      (this->_samples >> 1)) // To improve calculation on edge values
    interpolatedX =
        ((IndexOfMaxY + delta) * this->_samplingFrequency) / (this->_samples);
  // returned value: interpolated frequency peak apex
  return (interpolatedX);
}

void arduinoFFT::MajorPeak(double *f, double *v) {
  double maxY = 0;
  uint16_t IndexOfMaxY = 0;
  // If sampling_frequency = 2 * max_frequency in signal,
  // value would be stored at position samples/2
  for (uint16_t i = 1; i < ((this->_samples >> 1) + 1); i++) {
    if ((this->_vReal[i - 1] < this->_vReal[i]) &&
        (this->_vReal[i] > this->_vReal[i + 1])) {
      if (this->_vReal[i] > maxY) {
        maxY = this->_vReal[i];
        IndexOfMaxY = i;
      }
    }
  }
  double delta =
      0.5 *
      ((this->_vReal[IndexOfMaxY - 1] - this->_vReal[IndexOfMaxY + 1]) /
       (this->_vReal[IndexOfMaxY - 1] - (2.0 * this->_vReal[IndexOfMaxY]) +
        this->_vReal[IndexOfMaxY + 1]));
  double interpolatedX =
      ((IndexOfMaxY + delta) * this->_samplingFrequency) / (this->_samples - 1);
  if (IndexOfMaxY ==
      (this->_samples >> 1)) // To improve calculation on edge values
    interpolatedX =
        ((IndexOfMaxY + delta) * this->_samplingFrequency) / (this->_samples);
  // returned value: interpolated frequency peak apex
  *f = interpolatedX;
#if defined(ESP8266) || defined(ESP32)
  *v = fabs(this->_vReal[IndexOfMaxY - 1] - (2.0 * this->_vReal[IndexOfMaxY]) +
            this->_vReal[IndexOfMaxY + 1]);
#else
  *v = abs(this->_vReal[IndexOfMaxY - 1] - (2.0 * this->_vReal[IndexOfMaxY]) +
           this->_vReal[IndexOfMaxY + 1]);
#endif
}

double arduinoFFT::MajorPeak(double *vD, uint16_t samples,
                             double samplingFrequency) {
#warning("This method is deprecated and may be removed on future revisions.")
  double maxY = 0;
  uint16_t IndexOfMaxY = 0;
  // If sampling_frequency = 2 * max_frequency in signal,
  // value would be stored at position samples/2
  for (uint16_t i = 1; i < ((samples >> 1) + 1); i++) {
    if ((vD[i - 1] < vD[i]) && (vD[i] > vD[i + 1])) {
      if (vD[i] > maxY) {
        maxY = vD[i];
        IndexOfMaxY = i;
      }
    }
  }
  double delta =
      0.5 *
      ((vD[IndexOfMaxY - 1] - vD[IndexOfMaxY + 1]) /
       (vD[IndexOfMaxY - 1] - (2.0 * vD[IndexOfMaxY]) + vD[IndexOfMaxY + 1]));
  double interpolatedX =
      ((IndexOfMaxY + delta) * samplingFrequency) / (samples - 1);
  if (IndexOfMaxY == (samples >> 1)) // To improve calculation on edge values
    interpolatedX = ((IndexOfMaxY + delta) * samplingFrequency) / (samples);
  // returned value: interpolated frequency peak apex
  return (interpolatedX);
}

void arduinoFFT::MajorPeak(double *vD, uint16_t samples,
                           double samplingFrequency, double *f, double *v) {
#warning("This method is deprecated and may be removed on future revisions.")
  double maxY = 0;
  uint16_t IndexOfMaxY = 0;
  // If sampling_frequency = 2 * max_frequency in signal,
  // value would be stored at position samples/2
  for (uint16_t i = 1; i < ((samples >> 1) + 1); i++) {
    if ((vD[i - 1] < vD[i]) && (vD[i] > vD[i + 1])) {
      if (vD[i] > maxY) {
        maxY = vD[i];
        IndexOfMaxY = i;
      }
    }
  }
  double delta =
      0.5 *
      ((vD[IndexOfMaxY - 1] - vD[IndexOfMaxY + 1]) /
       (vD[IndexOfMaxY - 1] - (2.0 * vD[IndexOfMaxY]) + vD[IndexOfMaxY + 1]));
  double interpolatedX =
      ((IndexOfMaxY + delta) * samplingFrequency) / (samples - 1);
  // double popo =
  if (IndexOfMaxY == (samples >> 1)) // To improve calculation on edge values
    interpolatedX = ((IndexOfMaxY + delta) * samplingFrequency) / (samples);
  // returned value: interpolated frequency peak apex
  *f = interpolatedX;
#if defined(ESP8266) || defined(ESP32)
  *v =
      fabs(vD[IndexOfMaxY - 1] - (2.0 * vD[IndexOfMaxY]) + vD[IndexOfMaxY + 1]);
#else
  *v = abs(vD[IndexOfMaxY - 1] - (2.0 * vD[IndexOfMaxY]) + vD[IndexOfMaxY + 1]);
#endif
}

double arduinoFFT::MajorPeakParabola() {
  double maxY = 0;
  uint16_t IndexOfMaxY = 0;
  // If sampling_frequency = 2 * max_frequency in signal,
  // value would be stored at position samples/2
  for (uint16_t i = 1; i < ((this->_samples >> 1) + 1); i++) {
    if ((this->_vReal[i - 1] < this->_vReal[i]) &&
        (this->_vReal[i] > this->_vReal[i + 1])) {
      if (this->_vReal[i] > maxY) {
        maxY = this->_vReal[i];
        IndexOfMaxY = i;
      }
    }
  }

  double freq = 0;
  if (IndexOfMaxY > 0) {
    // Assume the three points to be on a parabola
    double a, b, c;
    Parabola(IndexOfMaxY - 1, this->_vReal[IndexOfMaxY - 1], IndexOfMaxY,
             this->_vReal[IndexOfMaxY], IndexOfMaxY + 1,
             this->_vReal[IndexOfMaxY + 1], &a, &b, &c);

    // Peak is at the middle of the parabola
    double x = -b / (2 * a);

    // And magnitude is at the extrema of the parabola if you want It...
    // double y = a*x*x+b*x+c;

    // Convert to frequency
    freq = (x * this->_samplingFrequency) / (this->_samples);
  }

  return freq;
}

void arduinoFFT::Parabola(double x1, double y1, double x2, double y2, double x3,
                          double y3, double *a, double *b, double *c) {
  double reversed_denom = 1 / ((x1 - x2) * (x1 - x3) * (x2 - x3));

  *a = (x3 * (y2 - y1) + x2 * (y1 - y3) + x1 * (y3 - y2)) * reversed_denom;
  *b = (x3 * x3 * (y1 - y2) + x2 * x2 * (y3 - y1) + x1 * x1 * (y2 - y3)) *
       reversed_denom;
  *c = (x2 * x3 * (x2 - x3) * y1 + x3 * x1 * (x3 - x1) * y2 +
        x1 * x2 * (x1 - x2) * y3) *
       reversed_denom;
}

uint8_t arduinoFFT::Exponent(uint16_t value) {
#warning("This method may not be accessible on future revisions.")
  // Calculates the base 2 logarithm of a value
  uint8_t result = 0;
  while (value >>= 1)
    result++;
  return (result);
}

// Private functions

void arduinoFFT::Swap(double *x, double *y) {
  double temp = *x;
  *x = *y;
  *y = temp;
}






















/*

	Example of use of the FFT library to compute FFT for a signal sampled through the ADC.
        Copyright (C) 2018 Enrique Condés and Ragnar Ranøyen Homb

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

arduinoFFT FFT;
/*
These values can be changed in order to evaluate the functions
*/
#define CHANNEL A0
const uint16_t samples = 64; //This value MUST ALWAYS be a power of 2
const double samplingFrequency = 100; //Hz, must be less than 10000 due to ADC

unsigned int sampling_period_us;
unsigned long microseconds;

/*
These are the input and output vectors
Input vectors receive computed results from FFT
*/
double vReal[samples];
double vImag[samples];

#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03

void setup()
{
  sampling_period_us = round(1000000*(1.0/samplingFrequency));
  Serial.begin(115200);
  while(!Serial);
  Serial.println("Ready");
}

void loop()
{
  /*SAMPLING*/
  microseconds = micros();
  for(int i=0; i<samples; i++)
  {
      vReal[i] = analogRead(CHANNEL);
      vImag[i] = 0;
      while(micros() - microseconds < sampling_period_us){
        //empty loop
      }
      microseconds += sampling_period_us;
  }
  FFT = arduinoFFT(vReal, vImag, samples, samplingFrequency); /* Create FFT object */
  /* Print the results of the sampling according to time */
  Serial.println("Data:");
  PrintVector(vReal, samples, SCL_TIME);
  FFT.Windowing(FFT_WIN_TYP_HAMMING, FFT_FORWARD);	/* Weigh data */
  Serial.println("Weighed data:");
  PrintVector(vReal, samples, SCL_TIME);
  FFT.Compute(FFT_FORWARD); /* Compute FFT */
  Serial.println("Computed Real values:");
  PrintVector(vReal, samples, SCL_INDEX);
  Serial.println("Computed Imaginary values:");
  PrintVector(vImag, samples, SCL_INDEX);
  FFT.ComplexToMagnitude(); /* Compute magnitudes */
  Serial.println("Computed magnitudes:");
  PrintVector(vReal, (samples >> 1), SCL_FREQUENCY);
  double x = FFT.MajorPeak();
  Serial.println(x, 6); //Print out what frequency is the most dominant.
  while(1); /* Run Once */
  // delay(2000); /* Repeat after delay */
}

void PrintVector(double *vData, uint16_t bufferSize, uint8_t scaleType)
{
  for (uint16_t i = 0; i < bufferSize; i++)
  {
    double abscissa;
    /* Print abscissa value */
    switch (scaleType)
    {
      case SCL_INDEX:
        abscissa = (i * 1.0);
	break;
      case SCL_TIME:
        abscissa = ((i * 1.0) / samplingFrequency);
	break;
      case SCL_FREQUENCY:
        abscissa = ((i * 1.0 * samplingFrequency) / samples);
	break;
    }
    Serial.print(abscissa, 6);
    if(scaleType==SCL_FREQUENCY)
      Serial.print("Hz");
    Serial.print(" ");
    Serial.println(vData[i], 4);
  }
  Serial.println();
}